{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.8.5-final"
    },
    "colab": {
      "name": "Robotic Manipulation - Basic Pick and Place.ipynb",
      "provenance": [],
      "collapsed_sections": [],
      "toc_visible": true
    }
  },
  "cells": [
    {
      "cell_type": "code",
      "metadata": {
        "id": "eeMrMI0-1Dhu",
        "colab_type": "code",
        "colab": {}
      },
      "source": [
        "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
        "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
        "proc, zmq_url, web_url = start_zmq_server_as_subprocess()\n",
        "\n",
        "# Let's do all of our imports here, too.\n",
        "import numpy as np\n",
        "\n",
        "from pydrake.all import (\n",
        "    AddMultibodyPlantSceneGraph, AngleAxis, BasicVector, ConnectMeshcatVisualizer, \n",
        "    DiagramBuilder, FindResourceOrThrow, Integrator, JacobianWrtVariable, \n",
        "    LeafSystem, MultibodyPlant, MultibodyPositionToGeometryPose, Parser, \n",
        "    PiecewisePolynomial, PiecewiseQuaternionSlerp, Quaternion, RigidTransform, \n",
        "    RollPitchYaw, RotationMatrix, SceneGraph, Simulator, TrajectorySource\n",
        ")\n",
        "from pydrake.examples.manipulation_station import ManipulationStation\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "QuFmomDOxoAR",
        "colab_type": "code",
        "colab": {},
        "tags": []
      },
      "source": [
        "builder = DiagramBuilder()\n",
        "\n",
        "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step = 0.0)\n",
        "parser = Parser(plant, scene_graph)\n",
        "grasp = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "    \"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"), \"grasp\")\n",
        "# TODO(russt): Draw the pregrasp gripper, too, as transparent (drake #13970).\n",
        "#pregrasp = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "#    \"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf\"), \"pregrasp\")\n",
        "brick = parser.AddModelFromFile(FindResourceOrThrow(\n",
        "    \"drake/examples/manipulation_station/models/061_foam_brick.sdf\"), \"brick\")\n",
        "plant.Finalize()\n",
        "\n",
        "frames_to_draw = {\"grasp\": {\"body\"},\n",
        "                  \"brick\": {\"base_link\"}}\n",
        "meshcat = ConnectMeshcatVisualizer(builder,\n",
        "    scene_graph,\n",
        "    zmq_url=zmq_url,\n",
        "    frames_to_draw=frames_to_draw,\n",
        "    axis_length=0.3,\n",
        "    axis_radius=0.01)\n",
        "\n",
        "diagram = builder.Build()\n",
        "context = diagram.CreateDefaultContext()\n",
        "plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "# TODO(russt): Set a random pose of the object.\n",
        "\n",
        "# Get the current object, O, pose\n",
        "B_O = plant.GetBodyByName(\"base_link\", brick)\n",
        "X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)\n",
        "\n",
        "B_Ggrasp = plant.GetBodyByName(\"body\", grasp)\n",
        "p_GgraspO = [0, 0.12, 0]\n",
        "R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply(\n",
        "    RotationMatrix.MakeZRotation(np.pi/2.0))\n",
        "# Useful for a better image:\n",
        "p_GgraspO = [0, 0.3, 0.1]\n",
        "R_GgraspO = R_GgraspO.multiply(RotationMatrix.MakeYRotation(0.5))\n",
        "X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)\n",
        "X_OGgrasp = X_GgraspO.inverse()\n",
        "X_WGgrasp = X_WO.multiply(X_OGgrasp)\n",
        "\n",
        "print(f\"p_GO_W = {X_WGgrasp.rotation().multiply(X_GgraspO.translation())}\")\n",
        "print(f\"p_GO_G = {X_GgraspO.translation()}\")\n",
        "print(f\"p_OG_O = {X_OGgrasp.translation()}\")\n",
        "\n",
        "plant.SetFreeBodyPose(plant_context, B_Ggrasp, X_WGgrasp)\n",
        "# Open the fingers, too.\n",
        "plant.GetJointByName(\"left_finger_sliding_joint\", grasp).set_translation(plant_context, -0.054)\n",
        "plant.GetJointByName(\"right_finger_sliding_joint\", grasp).set_translation(plant_context, 0.054)\n",
        "\n",
        "meshcat.load()\n",
        "diagram.Publish(context)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": []
    }
  ]
}